#include "Bujin.h"

// 全局常量定义
const float PULSES_PER_DEGREE = (STEPS_PER_REVOLUTION * MICROSTEPS) / 360.0f;

// --- X轴PID状态变量定义 ---
float pos_error_x = 0.0f;
float pos_last_error_x = 0.0f;
float pos_integral_x = 0.0f;
float pos_output_x = 0.0f;

float speed_error_x = 0.0f;
float speed_last_error_x = 0.0f;
float speed_integral_x = 0.0f;
float speed_output_x = 0.0f;
float actual_speed_x = 0.0f;

// --- Y轴PID状态变量定义 ---
float pos_error_y = 0.0f;
float pos_last_error_y = 0.0f;
float pos_integral_y = 0.0f;
float pos_output_y = 0.0f;

float speed_error_y = 0.0f;
float speed_last_error_y = 0.0f;
float speed_integral_y = 0.0f;
float speed_output_y = 0.0f;
float actual_speed_y = 0.0f;

// 脉冲计数变量定义
long x_pulse_count = 0;
long y_pulse_count = 0;

// 电机A状态变量定义
float s_ma_current_target_angle = -999.0f;
float s_ma_current_assumed_angle = 0.0f;
long  s_ma_pulses_remaining = 0;
int   s_ma_direction = 0;
float s_ma_pulse_accumulator = 0.0f;
int   s_ma_acceleration_counter = 0;
int   s_ma_decelerate_flag = 0;
int   s_ma_deceleration_counter = 0;

// 电机B状态变量定义
float s_mb_current_target_angle = -999.0f;
float s_mb_current_assumed_angle = 0.0f;
long s_mb_pulses_remaining = 0;
int s_mb_direction = 0;
float s_mb_pulse_accumulator = 0.0f;
int s_mb_acceleration_counter = 0;
int s_mb_decelerate_flag = 0;
int s_mb_deceleration_counter = 0;


void motor_control_init(void) {
    // 初始化GPIO和PWM(根据实际硬件配置)
    DL_GPIO_clearPins(EN_PORT, EN_MA_PIN | EN_MB_PIN);
    DL_TimerA_setLoadValue(PWM_0_INST, 0);
    DL_TimerG_setLoadValue(PWM_1_INST, 0);
}
// RPM转PWM周期值
static int rpm_to_pwm_period(int rpm) {
    if (rpm <= 0) return 0;

    float rps = (float)rpm / 60.0f;
    float steps_per_second = rps * STEPS_PER_REVOLUTION * MICROSTEPS;
    return (int)((PWM_CLOCK_FREQ / steps_per_second) - 1);
}

// 速度控制函数
int Calculate_target(int Target) {
    if (Target == 0) return 0;
    
    int direction = (Target >= 0) ? 1 : -1;
    float speed = (float)abs(Target);
    
    float stepsPerRevolution = STEPS_PER_REVOLUTION * MICROSTEPS;
    float stepsPerSecond = (speed * stepsPerRevolution) / 60.0f;
    float pwmFrequency = stepsPerSecond;
    
    uint16_t periodValue = (uint16_t)((PWM_CLOCK_FREQ / pwmFrequency) - 1);
    
    if (periodValue > 65535) {
        periodValue = 65535;
    }
    
    return direction * periodValue;
}

// 电机A角度控制
int Calculate_MA_Angle_PWM(float target_angle1) {
    float target_angle=-target_angle1;
    if (target_angle != s_ma_current_target_angle) {
        s_ma_current_target_angle = target_angle;
        float angle_to_move = target_angle - s_ma_current_assumed_angle;
        
        if (fabs(angle_to_move) < 0.1f) {
            s_ma_current_assumed_angle = target_angle;
            s_ma_pulses_remaining = 0;
            s_ma_direction = 0;
            s_ma_decelerate_flag = 0;
            s_ma_acceleration_counter = 0;
            s_ma_deceleration_counter = 0;
            return 0;
        }
        
        s_ma_pulses_remaining = (long)(fabs(angle_to_move) * PULSES_PER_DEGREE);
        s_ma_pulse_accumulator = 0.0f;
        s_ma_direction = (angle_to_move > 0) ? 1 : -1;
        s_ma_acceleration_counter = 0;
        s_ma_decelerate_flag = 0;
        s_ma_deceleration_counter = 0;
    }

    if (s_ma_pulses_remaining > 0) {
        int current_speed_rpm;
        float steps_per_second = (CONSTANT_SPEED_RPM * (STEPS_PER_REVOLUTION * MICROSTEPS)) / 60.0f;
        float pulses_per_isr = steps_per_second * ISR_INTERVAL_S;
        long total_deceleration_pulses = (long)(pulses_per_isr * DECELERATION_STEPS);

        if (s_ma_pulses_remaining <= total_deceleration_pulses) {
            s_ma_decelerate_flag = 1;
        }

        if (!s_ma_decelerate_flag && s_ma_acceleration_counter < ACCELERATION_STEPS) {
            float acceleration_progress = (float)s_ma_acceleration_counter / ACCELERATION_STEPS;
            current_speed_rpm = (int)(START_SPEED_RPM + (CONSTANT_SPEED_RPM - START_SPEED_RPM) * acceleration_progress * acceleration_progress);
            s_ma_acceleration_counter++;
        }
        else if (s_ma_decelerate_flag) {
            float deceleration_progress = (float)s_ma_deceleration_counter / DECELERATION_STEPS;
            current_speed_rpm = (int)(CONSTANT_SPEED_RPM * (1 - deceleration_progress));
            if (current_speed_rpm < START_SPEED_RPM) {
                current_speed_rpm = START_SPEED_RPM;
            }
            s_ma_deceleration_counter++;
        }
        else {
            current_speed_rpm = CONSTANT_SPEED_RPM;
        }

        steps_per_second = (current_speed_rpm * (STEPS_PER_REVOLUTION * MICROSTEPS)) / 60.0f;
        float pulses_to_send_float = steps_per_second * ISR_INTERVAL_S;
        s_ma_pulse_accumulator += pulses_to_send_float;
        long pulses_this_interval = (long)s_ma_pulse_accumulator;
        
        if (pulses_this_interval > 0) {
            s_ma_pulse_accumulator -= pulses_this_interval;
            s_ma_pulses_remaining -= pulses_this_interval;
            x_pulse_count += pulses_this_interval; // 累计脉冲数
        }

        if (s_ma_pulses_remaining <= 0) {
            s_ma_pulses_remaining = 0;
            s_ma_direction = 0;
            s_ma_decelerate_flag = 0;
            s_ma_acceleration_counter = 0;
            s_ma_deceleration_counter = 0;
            s_ma_current_assumed_angle = s_ma_current_target_angle;
            return 0;
        }
        
        return s_ma_direction * rpm_to_pwm_period(current_speed_rpm);
    }
    
    return 0;
}

// 电机B角度控制
int Calculate_MB_Angle_PWM(float target_angle) {
    if (target_angle != s_mb_current_target_angle) {
        s_mb_current_target_angle = target_angle;
        float angle_to_move = target_angle - s_mb_current_assumed_angle;
        
        if (fabs(angle_to_move) < 0.1f) {
            s_mb_current_assumed_angle = target_angle;
            s_mb_pulses_remaining = 0;
            s_mb_direction = 0;
            s_mb_decelerate_flag = 0;
            s_mb_acceleration_counter = 0;
            s_mb_deceleration_counter = 0;
            return 0;
        }
        
        s_mb_pulses_remaining = (long)(fabs(angle_to_move) * PULSES_PER_DEGREE);
        s_mb_pulse_accumulator = 0.0f;
        s_mb_direction = (angle_to_move > 0) ? 1 : -1;
        s_mb_acceleration_counter = 0;
        s_mb_decelerate_flag = 0;
        s_mb_deceleration_counter = 0;
    }

    if (s_mb_pulses_remaining > 0) {
        int current_speed_rpm;
        float steps_per_second = (CONSTANT_SPEED_RPM * (STEPS_PER_REVOLUTION * MICROSTEPS)) / 60.0f;
        float pulses_per_isr = steps_per_second * ISR_INTERVAL_S;
        long total_deceleration_pulses = (long)(pulses_per_isr * DECELERATION_STEPS);

        if (s_mb_pulses_remaining <= total_deceleration_pulses) {
            s_mb_decelerate_flag = 1;
        }

        if (!s_mb_decelerate_flag && s_mb_acceleration_counter < ACCELERATION_STEPS) {
            float acceleration_progress = (float)s_mb_acceleration_counter / ACCELERATION_STEPS;
            current_speed_rpm = (int)(START_SPEED_RPM + (CONSTANT_SPEED_RPM - START_SPEED_RPM) * acceleration_progress * acceleration_progress);
            s_mb_acceleration_counter++;
        }
        else if (s_mb_decelerate_flag) {
            float deceleration_progress = (float)s_mb_deceleration_counter / DECELERATION_STEPS;
            current_speed_rpm = (int)(CONSTANT_SPEED_RPM * (1 - deceleration_progress));
            if (current_speed_rpm < START_SPEED_RPM) {
                current_speed_rpm = START_SPEED_RPM;
            }
            s_mb_deceleration_counter++;
        }
        else {
            current_speed_rpm = CONSTANT_SPEED_RPM;
        }

        steps_per_second = (current_speed_rpm * (STEPS_PER_REVOLUTION * MICROSTEPS)) / 60.0f;
        float pulses_to_send_float = steps_per_second * ISR_INTERVAL_S;
        s_mb_pulse_accumulator += pulses_to_send_float;
        long pulses_this_interval = (long)s_mb_pulse_accumulator;
        
        if (pulses_this_interval > 0) {
            s_mb_pulse_accumulator -= pulses_this_interval;
            s_mb_pulses_remaining -= pulses_this_interval;
            y_pulse_count += pulses_this_interval; // 累计脉冲数
        }

        if (s_mb_pulses_remaining <= 0) {
            s_mb_pulses_remaining = 0;
            s_mb_direction = 0;
            s_mb_decelerate_flag = 0;
            s_mb_acceleration_counter = 0;
            s_mb_deceleration_counter = 0;
            s_mb_current_assumed_angle = s_mb_current_target_angle;
            return 0;
        }
        
        return s_mb_direction * rpm_to_pwm_period(current_speed_rpm);
    }
    
    return 0;
}


// 更新实际转速
void Update_Actual_Speed(void) {
    // 计算X轴实际转速(RPM)
    float x_revolutions = (float)x_pulse_count / (STEPS_PER_REVOLUTION * MICROSTEPS);
    actual_speed_x = x_revolutions / (ISR_INTERVAL_S / 60.0f);
    x_pulse_count = 0;
    
    // 计算Y轴实际转速
    float y_revolutions = (float)y_pulse_count / (STEPS_PER_REVOLUTION * MICROSTEPS);
    actual_speed_y = y_revolutions / (ISR_INTERVAL_S / 60.0f);
    y_pulse_count = 0;
}

// 设置PWM输出
void Set_PWM(int L_Target, int R_Target) {
    // 左电机控制
    if (L_Target > 0) {
        DL_GPIO_setPins(EN_PORT, EN_MA_PIN);
        DL_GPIO_setPins(DIR_PORT, DIR_A_PIN);
        DL_TimerA_setLoadValue(PWM_0_INST, abs(L_Target));
        DL_Timer_setCaptureCompareValue(PWM_0_INST, abs(L_Target/2), GPIO_PWM_0_C3_IDX);
    } else if (L_Target < 0) {
        DL_GPIO_setPins(EN_PORT, EN_MA_PIN);
        DL_GPIO_clearPins(DIR_PORT, DIR_A_PIN);
        DL_TimerA_setLoadValue(PWM_0_INST, abs(L_Target));
        DL_Timer_setCaptureCompareValue(PWM_0_INST, abs(L_Target/2), GPIO_PWM_0_C3_IDX);
    } else {
        DL_GPIO_clearPins(EN_PORT, EN_MA_PIN);
    }

    // 右电机控制
    if (R_Target > 0) {
        DL_GPIO_setPins(EN_PORT, EN_MB_PIN);
        DL_GPIO_setPins(DIR_PORT, DIR_B_PIN);
        DL_TimerG_setLoadValue(PWM_1_INST, abs(R_Target));
        DL_Timer_setCaptureCompareValue(PWM_1_INST, abs(R_Target/2), GPIO_PWM_1_C1_IDX);
    } else if (R_Target < 0) {
        DL_GPIO_setPins(EN_PORT, EN_MB_PIN);
        DL_GPIO_clearPins(DIR_PORT, DIR_B_PIN);
        DL_TimerG_setLoadValue(PWM_1_INST, abs(R_Target));
        DL_Timer_setCaptureCompareValue(PWM_1_INST, abs(R_Target/2), GPIO_PWM_1_C1_IDX);
    } else {
        DL_GPIO_clearPins(EN_PORT, EN_MB_PIN);
    }
}
